function F = objfun101(x,feedback,settings)
% F = objfun101(x,feedback,settings)
% Objective function for MPC101. Penalizes deviation of roughness, height
% and SOR

% A constant table
Tab_D_DepRate   = [0.1,0.2,0.5,1.0];
Tab_D_K         = [0.99962,0.9914,0.9730,0.9532];
Tab_D_Tau       = [10.0740,5.3215,2.1915,1.1525];

Tab_R_DepRate   = [0.10,0.15,0.20,0.50,1.0];
Tab_R_nu        = [0.0451e-4,1.3915e-05,0.3583e-4,0.1232e-3,0.3939e-3];
Tab_R_sigma2    = [1.0810e-3,6.2341e-3,2.1983e-2,0.0990,0.3007];
Tab_R_Rh        = [0.1002,0.1509,0.2021,0.5129,1.0432];

% assert(length(x) == settings.P,'\nERROR@objfun: length(x) should be 5.');

if nargin == 2
    D_set       = 0.99;
    Fact_D      = 0.0;
    R2_set      = 50;
    Fact_r2     = 1.0;
    Ht_set      = 800;
    Fact_H      = 1.0;
    P           = 5;  % Prediction steps
    m           = 20; % Mode
    dt          = 10;
elseif nargin == 3
%     assert(isstruct(settings),'\nsettings should be a struct');
    D_set   = settings.D_set;
    Fact_D  = settings.Fact_D;
    R2_set  = settings.R2_set;
    Fact_r2 = settings.Fact_r2;
    Ht_set  = settings.Ht_set;
    Fact_H  = settings.Fact_H;
    P = settings.P;
    m = settings.m;
    dt = settings.dt;
else
    error('\nThere must be two or three input');
end

% assert(isstruct(feedback),'\nfeedback should be a struct');
rho         = zeros(P+1,1);
h           = zeros(P+1,1);
alpha2      = zeros(P+1,m);
beta2       = zeros(P+1,m);

rho(1)      = feedback.rho;
h(1)        = feedback.h;
alpha2(1,:) = (feedback.alpha).^2;
beta2(1,:)  = (feedback.beta).^2;

F = 0;
for i = 1:P
    if Tab_D_DepRate(1) < x(i) <= Tab_D_DepRate(end)
        K = interp1(Tab_D_DepRate,Tab_D_K,x(i),'linear','extrap');
        tau = interp1(Tab_D_DepRate,Tab_D_Tau,x(i),'linear','extrap');
    elseif x(i) <= Tab_D_DepRate(1)
        K = Tab_D_K(1);
        tau = Tab_D_Tau(1);
    else
        K = Tab_D_K(end);
        tau = Tab_D_Tau(end);
    end
    
    if Tab_R_DepRate(1) <x(i) <= Tab_R_DepRate(end)
        nu = interp1(Tab_R_DepRate,Tab_R_nu,x(i),'linear','extrap');
        sigma2 = interp1(Tab_R_DepRate,Tab_R_sigma2,x(i),'linear','extrap');
        Rh = interp1(Tab_R_DepRate,Tab_R_Rh,x(i),'linear','extrap');
    elseif x(i) <= Tab_R_DepRate(1)
        nu = Tab_R_nu(1);
        sigma2 = Tab_R_sigma2(1);
        Rh = Tab_R_Rh(1);
    else
        nu = Tab_R_nu(end);
        sigma2 = Tab_R_sigma2(end);
        Rh = Tab_R_Rh(end);
    end
    
    % Debugging code
%     fprintf(1,'K = %f\ntau = %f\n nu = %f\nsigma2 = %f\nrh = %f\n',K,tau,nu,sigma2,Rh);

    % Thickness model
    h(i+1) = h(i)+Rh*dt;
    if h(i+1) < Ht_set
        cost_H = ((h(i+1)-Ht_set)/Ht_set)^2;
    else
        cost_H = 0;
    end

    % SOR model
    rho(i+1) = (rho(i)*h(i)+Rh*(K*dt+(K-rho(i))*tau*(exp(-dt/tau)-1)))/(h(i)+Rh*dt);

    % Roughness model
    for j = 1:m
        temp = sigma2/(2*nu*j^2);
        temp2 = exp(-2*nu*j^2*dt);
        alpha2(i+1,j) = temp+(alpha2(i,j)-temp)*temp2;
        beta2(i+1,j) = temp+(beta2(i,j)-temp)*temp2;
    end
    r2 = sum(alpha2(i+1,:)+beta2(i+1,:))/(2*pi);

    % Another method to calculate r2: using function model_R2
%     [meanAlpha2,meanBeta2,varAlpha2,varBeta2,meanR2(i),varR2(i)] = model_R2(x(i),meanAlpha2,meanBeta2,varAlpha2,varBeta2,dt,model_R2_settings);
   
    F = F+Fact_D*((D_set-rho(i+1))/D_set)^2+Fact_r2*((R2_set-r2)/R2_set)^2 + Fact_H*cost_H;

    % Debugging code
%     fprintf(1,'((r2-r2_set)/r2_set)^2 = %f\n',((R2_set-r2)/R2_set)^2);
%     fprintf(1,'((h-h_set)/h_set)^2 = %f\n',cost_H);
%     fprintf(1,'((SOR-SOR_set)/SOR_set)^2 = %f\n',((D_set-rho(i+1))/D_set)^2);
%     fprintf(1,'P = %d, cost(total) = %f\n',i,cost);    
%     F = F+Fact_D*((D_set-rho(i+1))/D_set)^2+Fact_r2*((R2_set-r2)/R2_set)^2 + Fact_H*cost_H;
end
